import { SMap } from "./SMap";
import Agent from "./Agent";
import KdTree from "./KdTree";
import Obstacle from "./Obstacle";
import RVOMath from "./RVOMath";
import Vector2 from "./Vector2";
import { MathUtils } from "../MathUtils";
import { IVec2Like } from "cc";


export default class Simulator {
    private static _instance: Simulator;
    public static get Instance() {
        if (!Simulator._instance) {
            Simulator._instance = new Simulator();
        }
        return Simulator._instance;
    }

    public s_totalID = 0;

    agentMap = new SMap<number, Agent>();

    public obstacles: Array<Obstacle> = [];
    public kdTree_: KdTree;
    public timeStep_: number;

    public defaultAgent: Agent;
    public globalTime: number;

    private _change = false;

    constructor() {
        this.init();
    }

    private init() {
        this.defaultAgent = null;
        this.kdTree_ = new KdTree(this);
        this.obstacles.length = 0;
        this.globalTime = 0;
        this.timeStep_ = 0.1;
    }

    clear() {
        let t = this;
        t.agentMap.clear();
        t._change = false;
        t.kdTree_ = new KdTree(this);
        t.obstacles.length = 0;
        t.globalTime = 0;
        t.timeStep_ = 0.1;
    }

    clearObstacles() {
        this.obstacles.length = 0
    }

    public step(dt: number) {
        let t = this;
        let t_change = true;
        if (t._change) {
            t._change = false;
            t_change = true;
        }

        t.kdTree_.buildAgentTree(t_change);

        let t_agents = t.agentMap.values();

        for (let i = 0, j = t_agents.length; i < j; i++) {
            let agent = t_agents[i];
            agent.computeNeighbors();
            agent.computeNewVelocity();
        }

        for (let i = 0, j = t_agents.length; i < j; i++) {
            let agent = t_agents[i];
            agent.update();
        }

        t.globalTime = MathUtils.fixNumber(t.globalTime + dt);
        return t.globalTime;
    }

    public createAgent(position: IVec2Like, pCfg: AgentCfg = null) {
        if (!pCfg && !this.defaultAgent)
            return -1;
        let agent = new Agent(this);

        if (pCfg) {
            agent.maxNeighbors = pCfg.maxNeighbors;
            agent.maxSpeed = pCfg.maxSpeed;
            agent.neighborDist = pCfg.neighborDist;
            agent.position = position;
            agent.radius = pCfg.radius;
            agent.timeHorizon = pCfg.timeHorizon;
            agent.timeHorizonObst = pCfg.timeHorizonObst;
            agent.velocity = pCfg.velocity;
            agent.mass = pCfg.mass;
        }
        else {
            agent.maxNeighbors = this.defaultAgent.maxNeighbors;
            agent.maxSpeed = this.defaultAgent.maxSpeed;
            agent.neighborDist = this.defaultAgent.neighborDist;
            agent.position = position;
            agent.radius = this.defaultAgent.radius;
            agent.timeHorizon = this.defaultAgent.timeHorizon;
            agent.timeHorizonObst = this.defaultAgent.timeHorizonObst;
            agent.velocity = this.defaultAgent.velocity;
            agent.mass = this.defaultAgent.mass;
        }
        return this.addAgent(agent);
    }

    public addAgent(agent: Agent) {
        agent.id = this.s_totalID;
        this.s_totalID++;
        this.agentMap.set(agent.id, agent);
        this._change = true;
        return agent.id;
    }

    removeAgent(pAgentId: number) {
        let t = this;
        if (t.agentMap.has(pAgentId)) {
            t.agentMap.delete(pAgentId);
            t._change = true;
        }
    }
    getAgentByAid(aid: number) {
        return this.agentMap.get(aid);
    }
    public addObstacle(vertices: Array<Vector2>) {
        if (vertices.length < 2) return -1;

        let obstacleNo = this.obstacles.length;
        for (let i = 0; i < vertices.length; ++i) {
            let obstacle = new Obstacle();
            obstacle.point_ = vertices[i];

            if (i != 0) {
                obstacle.previous_ = this.obstacles[this.obstacles.length - 1];
                obstacle.previous_.next_ = obstacle;
            }

            if (i == vertices.length - 1) {
                obstacle.next_ = this.obstacles[obstacleNo];
                obstacle.next_.previous_ = obstacle;
            }

            obstacle.direction_ = RVOMath.normalize(Vector2.subtract(vertices[(i == vertices.length - 1 ? 0 : i + 1)], vertices[i]));

            if (vertices.length == 2) {
                obstacle.convex_ = true;
            } else {
                obstacle.convex_ = (RVOMath.leftOf(vertices[(i == 0 ? vertices.length - 1 : i - 1)], vertices[i], vertices[(i == vertices.length - 1 ? 0 : i + 1)]) >= 0);
            }

            obstacle.id_ = this.obstacles.length;
            this.obstacles.push(obstacle);
        }

        return obstacleNo;
    }

    public getAgentPosition(agentId: number) {
        let agent = this.agentMap.get(agentId);
        if (agent) {
            return agent.position;
        } else {
            return new Vector2(0, 0);
        }
    }

    public getAgentPrefVelocity(agentId: number) {
        let t = this;
        let t_agent = t.agentMap.get(agentId);
        if (t_agent) {
            return t_agent.prefVelocity;
        }
    }

    public setTimeStep(timeStep: number) {
        this.timeStep_ = timeStep;
    }

    public setAgentDefaults(
        neighborDist: number,
        maxNeighbors: number,
        timeHorizon: number,
        timeHorizonObst: number,
        radius: number,
        maxSpeed: number,
        velocity: Vector2,
        mass: number,
    ) {
        if (this.defaultAgent == null) {
            this.defaultAgent = new Agent(this);
        }

        this.defaultAgent.maxNeighbors = maxNeighbors;
        this.defaultAgent.maxSpeed = maxSpeed;
        this.defaultAgent.neighborDist = neighborDist;
        this.defaultAgent.radius = radius;
        this.defaultAgent.timeHorizon = timeHorizon;
        this.defaultAgent.timeHorizonObst = timeHorizonObst;
        this.defaultAgent.velocity = velocity;
        this.defaultAgent.mass = mass;

        return this.defaultAgent;
    }

    public processObstacles() {
        this.kdTree_.buildObstacleTree();
    }

    public setAgentPrefVelocity(agentId: number, prefVelocity: Vector2) {
        let t = this;
        let t_agent = t.agentMap.get(agentId);
        if (t_agent) {
            t_agent.prefVelocity = prefVelocity;
        }
    }
}

export class AgentCfg {
    public neighborDist: number;
    public maxNeighbors: number;
    public timeHorizon: number;
    public timeHorizonObst: number;
    public radius: number;
    public maxSpeed: number;
    public velocity: Vector2;
    mass: number;

    speedFactor: number = 1;

    constructor(
        neighborDist?: number,
        maxNeighbors?: number,
        timeHorizon?: number,
        timeHorizonObst?: number,
        radius?: number,
        maxSpeed?: number,
        velocity?: Vector2,
        mass?: number
    ) {
        if (neighborDist != undefined)
            this.neighborDist = neighborDist;
        if (maxNeighbors != undefined)
            this.maxNeighbors = maxNeighbors;
        if (timeHorizon != undefined)
            this.timeHorizon = timeHorizon;
        if (timeHorizonObst != undefined)
            this.timeHorizonObst = timeHorizonObst;
        if (radius != undefined)
            this.radius = radius;
        if (maxSpeed != undefined)
            this.maxSpeed = maxSpeed;
        if (velocity != undefined)
            this.velocity = velocity;
        if (mass != undefined)
            this.mass = mass;
    }

    copyFromAgent(pAgent: Agent) {
        let t = this;
        t.neighborDist = pAgent.neighborDist;
        t.maxNeighbors = pAgent.maxNeighbors;
        t.timeHorizon = pAgent.timeHorizon;
        t.timeHorizonObst = pAgent.timeHorizonObst;
        t.radius = pAgent.radius;
        t.maxSpeed = pAgent.maxSpeed;
        t.velocity = pAgent.velocity;
        t.mass = pAgent.mass;
    }
}